package com.rockwell.sniffyhunter.model;

import com.rockwell.sniffyhunter.utils.IRobotPositionDetector;
import com.rockwell.sniffyhunter.utils.IStateMachine;
import com.rockwell.sniffyhunter.utils.TreasureUnloaderState;

/**
 * This class provides the robot for unloading treasure
 * in the maze.
 * The following actions will be performed:
 * <ul>
 * <li>Move to the unloading position.
 * <li>U Turn back.
 * <li>Unload treasure.
 * </ul>
 * <p>
 * 
 * @author      Sniffy Software Team
 * @version     1.0
 */

public class TreasureUnloader extends StateObject {
	
	private static int MOVE_INDEX = 20; //mm
	private static int MOVE_INDEX2 = 50; //mm
	private RobotCommandSequencer m_commandSequencer;
	private RobotPosition m_robotPosition;
	private IRobotPositionDetector m_positionDetector;

	public TreasureUnloader(Robot robot) {
		super(robot);
		m_commandSequencer = new RobotCommandSequencer(robot);
		m_positionDetector = robot.m_positionDetector;
	}
	
	@Override
	public void Initialize() {
		//m_commandSequencer.Initialize();
		machineState = TreasureUnloaderState.START;
		machineRecoveryState = machineState;
	}
	
	@Override
	public boolean IsDone() {
		return machineState == TreasureUnloaderState.DONE;
	}

	public IStateMachine moveToUnloadPos() {

		ReturnResult returnVal = m_commandSequencer.PerformAction(RobotAction.Move2, MOVE_INDEX);

		if (returnVal == ReturnResult.Completed) {
			log("moveIntoJunction.TreasureUnloader", 4);
			machineState = TreasureUnloaderState.CHECK_MOVE_TO_UNLOAD_POS_DONE;
		}
		
		return machineState;
	}
	
	public IStateMachine checkMoveToUnloadPosDone() {

		m_robotPosition = m_positionDetector.getPosition();
		
		boolean isInUnloadPos = true;
		//TODO Get treasure position and check

		if (isInUnloadPos) {
			log("isInUnloadPos = true.TreasureUnloader", 4);
			machineState = TreasureUnloaderState.U_TURN;
		}
		else {
			log("isInUnloadPos = false.TreasureUnloader", 4);
			machineState = TreasureUnloaderState.MOVE_TO_UNLOAD_POS;
		}
		
		return machineState;
	}
	
	public IStateMachine uTurn() {

		ReturnResult returnVal = m_commandSequencer.PerformAction(RobotAction.TurnBack);

		if (returnVal == ReturnResult.Completed) {
			log("uTurn.TreasureUnloader", 4);
			machineState = TreasureUnloaderState.MOVE_FORWARD_BEFORE_UNLOAD;
		}
		
		return machineState;
	}
	
	public IStateMachine moveForwardBeforeUnload() {

		ReturnResult returnVal = m_commandSequencer.PerformAction(RobotAction.Move2, MOVE_INDEX2);

		if (returnVal == ReturnResult.Completed) {
			log("moveForwardBeforeUnload.TreasureUnloader", 4);
			machineState = TreasureUnloaderState.UNLOAD;
		}
		
		return machineState;
	}
	
	public IStateMachine unload() {

		ReturnResult returnVal = m_commandSequencer.PerformAction(RobotAction.Unload);

		if (returnVal == ReturnResult.Completed) {
			log("unload.TreasureUnloader", 4);
			robot.resetTreasurePickCount();
			machineState = TreasureUnloaderState.DONE;
		}
		
		return machineState;
	}
}